package jp.seraph.jspf.pose;

import java.util.ArrayList;
import java.util.HashSet;
import java.util.Iterator;
import java.util.List;
import java.util.Set;
import java.util.Map.Entry;

import jp.seraph.jsade.math.Angle;
import jp.seraph.jsade.model.JointIdentifier;
import jp.seraph.jsade.task.AgentTask;
import jp.seraph.jspf.message.MessageUtil;

/**
 * Identifierの重複を許さないPoseSet とりあえずのデフォルト実装。
 * 矛盾を起こさないために、このセットに含まれるPoseのJointIdentifierは重複してはならない
 *
 */
public class DefaultPoseSet implements PoseSet {
    public DefaultPoseSet() {
        mIdentifierSet = new HashSet<JointIdentifier>();
        mPoseElements = new ArrayList<PoseElement>();
    }

    public DefaultPoseSet(PoseElement... aElements){
        this();

        for(PoseElement tElement : aElements)
            this.addElement(tElement);
    }

    private Set<JointIdentifier> mIdentifierSet;
    private List<PoseElement> mPoseElements;

    public void addElement(PoseElement aElement) {
        Pose tPose = aElement.getPose();
        for (Entry<JointIdentifier, Angle> tEntry : tPose) {
            if (mIdentifierSet.contains(tEntry.getKey()))
                throw new IllegalArgumentException(MessageUtil.jointIdentifierDuplicated(tEntry.getKey()));

            mIdentifierSet.add(tEntry.getKey());
        }

        mPoseElements.add(aElement);
    }

    public PoseElement getElement(int aIndex) {
        return mPoseElements.get(aIndex);
    }

    public int getElementCount() {
        return mPoseElements.size();
    }

    public Iterator<PoseElement> iterator() {
        return mPoseElements.iterator();
    }

    /**
     *
     * @see jp.seraph.jspf.pose.PoseSet#toTask()
     */
    public AgentTask toTask() {
        return new PoseTask(this);
    }
}
